﻿#ifndef MEMOTION_H
#define MEMOTION_H

#include <iostream>
#include <vector>

/**
 * @brief 运动插值等的通用数学算法，注意和后面的motion模块区分开来
 */
class MeMotion
{
public:
    /**
     * 定义插值回调函数, 处理得到的绝对和相对点
     * passObject 传递对象指针
     * passData 需要传递的数据 lineBresenham 时候用到
     * 这个函数在调用
     */
    typedef void(*lpInterpolationCB)(void *passObject, void *passData, std::vector<int> *absPos, std::vector<int> *oneStep);

    /**
     * @brief Bresenham 直线插值算法,支持2-n维的算法
     * @param spos 起点
     * @param epos 终点
     * @param size 起点和终点的维度
     * @return 返回线性插值点
     */
    static std::vector<std::vector<int>> lineBresenham(int *spos, int *epos, const unsigned int &size);
    /// Bresenham 直线插值算法,支持2-n维的算法
    /// factor 是放大比例，插值是按照单位步长进行的，相当于步长为 1/factor。最好使用10的倍数
    ///        比如 0-1mm, factor=10的时候，就是0-10之间按照step=1插值。
    /// @return 返回线性插值点
    static std::vector<std::vector<double>> lineBresenham(double *sposF, double *eposF, const int factor, const unsigned int &size);
    /// Bresenham 线性插值算法，插值的结果在fCB回调函数处理。
    /// 其中 passObject , passData 分别代表透传的对象指针和数据指针
    static void lineBresenham(const int *spos, const int *epos, const unsigned int &size,
                              lpInterpolationCB fCB, void *passObject, void *passData);

    /**
     * @brief 简单的线性插值算法：根据count来计算插入点的数量(浮点计算，效率慢)
     * @param spos 起点坐标
     * @param epos 终点坐标
     * @param count 需要分成多少个点（包括起点和终点，取值要>=2, 等于2的时候实际不做任何事情）
     * @param dimension 维度
     * @return 所有的插补点
     */
    static std::vector<std::vector<double>> linearPoints(
            double *spos, double *epos, const size_t count, const size_t dimension=3);
    /**
     * @brief 简单的线性插值算法：根据步长来计算插入点的数量(浮点计算，效率慢)
     * @param spos 起点坐标
     * @param epos 终点坐标
     * @param step 步长,单位mm
     * @param dimension 维度
     * @return 所有的插补点
     */
    static std::vector<std::vector<double>> linearPointsWithStep(
            double *spos, double *epos, const double step, const size_t dimension=3);

    /**
     * @brief 二维弧度插值算法
     * @param current 起始点
     * @param target 终点
     * @param center 中心点的坐标
     * @param isClockwiseArc 顺弧度 or 逆弧度
     */
    static std::vector<std::vector<int>> arc2DPoints(
            int *current, int *target, int *center,
            bool isClockwiseArc
            );
    /// 二维弧度插值算法: 通过传入起点 current， 终点 target， 圆心 center 插补圆弧数据
    /// factor 是放大比例，插值是按照单位步长进行的，相当于步长为 1/factor。
    ///        比如 0-1mm, factor=10的时候，就是0-10之间按照step=1插值。
    static std::vector<std::vector<double>> arc2DPoints(
            double *current, double *target, double *center,
            const int factor,
            bool isClockwiseArc
            );
    /// 二维弧度插值算法: 插值的起点(Xs,Ys), 终点(Xe, Ye), 圆心(0,0), 半径为 radius 二维圆弧插值
    /// 返回：插补的点的集合(包括起点和终点)
    static std::vector<std::vector<int>> arc2DPoints(
            const int &Xs, const int &Ys, const int &Xe, const int &Ye,
            const int &radius,
            bool isClockwiseArc
            );

    /// 三点圆弧插值(3D): p1为起点， 经过p2, 到达p3
    /// factor 是放大比例，插值是按照单位步长进行的，相当于步长为 1/factor。
    ///        比如 0-1mm, factor=10的时候，就是0-10之间按照step=1插值。
    /// 返回值: 0 - rad, -1 数据又问题， -2 三点共线。
    static int arc3DPoints(
            double p1[3], double p2[3], double p3[3],
            const int factor, std::vector<std::vector<double>> &outPoints);

    /// Bresenhame二维圆的插值算法.  输出的点从XY(0, radius)点开始，顺时针插补圆上所有的点。
    /// 参考: Bresenham圆的生成算法： https://blog.csdn.net/qq_41698119/article/details/101224454
    static std::vector<std::vector<int>> circlePoints(int radius);

    /// 三维空间，通过三点圆弧求中心点
    /// p1, p2, p3 为输入的3点，center 为输出的中心点
    /// 返回值: 0 - rad, -1 数据又问题， -2 三点共线
    static int circle3DCenter(
            const double p1[3], const double p2[3], const double p3[3], double center[3]);

    /// 圆弧插值，编写gcode弧度插值编写的算法
    /// TODO: 移植老的代码，最近没验证
    std::vector<std::vector<long> > arc2DInterpolationPoints(
            long *current, long *target, long *center, bool isClockwiseArc, long step);

private:
    /**
     * @brief 对于XY平面，往前走一步
     * @param f 偏差，比较X，Y方向，往偏差小的方向走
     * @param signXs X运动方向
     * @param signYs X运动方向
     * @param Xi 当前运动到的点的X坐标
     * @param Yi 当前运动到的点的Y坐标
     */
    static void arcStepOne(int &f, int8_t &signXs, int8_t &signYs, long &Xi, long &Yi);

    /// 平面内，判断是否接近最近的插值点(2D)。
    /// 由于插补可能产生的偏差，插值的点和目标点可能存在一个单位的误差。
    /// 在判断插值点是否达到目的点非常有用。
    static bool isNearArcPoint(const int &x, const int &y, const int &tx, const int &ty, const bool &isClockwiseArc);
};

#endif // MEMOTION_H
